#include "ConstraintSystem.h"

ConstraintSystem::ConstraintSystem(void)
{
}

ConstraintSystem::ConstraintSystem( double alpha, double beta )
{
	this->alpha=alpha;
	this->beta=beta;
}

ConstraintSystem::~ConstraintSystem(void)
{
	Clear();
}

void ConstraintSystem::Clear()
{
	for (int i=0;i<constraints.size();i++)
	{
		delete constraints[i];
	}

	constraints.clear();
}

void ConstraintSystem::AddConstraint( Constraint *c )
{
	constraints.push_back(c);
	c->index=constraints.size()-1;
}

void ConstraintSystem::GetConstraint( double *constraint )
{
	for (int i=0;i<constraints.size();i++)
	{
		Particle *a=constraints[i]->jun;
		Particle *b=constraints[i]->sen;

		double xd=b->pos[0]-a->pos[0];
		double yd=b->pos[1]-a->pos[1];
		constraint[i]=xd*xd+yd*yd-constraints[i]->distance*constraints[i]->distance;
	}
}

void ConstraintSystem::GetConstraintDeriv( double *consDe )
{
	for (int i=0;i<constraints.size();i++)
	{
		Particle *a=constraints[i]->jun;
		Particle *b=constraints[i]->sen;

		consDe[i]=2*((b->pos[0]-a->pos[0])*(b->vel[0]-a->vel[0])+(b->pos[1]-a->pos[1])*(b->vel[1]-a->vel[1]));
	}
}

void ConstraintSystem::GetConstraintJacobian( double **jacobian, ParticleList *particles )
{
	for (int i=0;i<constraints.size();i++)
	{
		int index=0;
		Particle *a=constraints[i]->jun;
		Particle *b=constraints[i]->sen;

		for (int j=0;j<particles->size();j++)
		{
			if (j==a->index)
			{
				jacobian[i][index++]=2*(a->pos[0]-b->pos[0]);
				jacobian[i][index++]=2*(a->pos[1]-b->pos[1]);
			}else if (j==b->index)
			{
				jacobian[i][index++]=2*(b->pos[0]-a->pos[0]);
				jacobian[i][index++]=2*(b->pos[1]-a->pos[1]);
			}else{
				jacobian[i][index++]=0;
				jacobian[i][index++]=0;
			}
		}
	}
}

void ConstraintSystem::GetConstraintJacobianDerivVector( double *jacobianV, ParticleList *particles )
{
	for (int i=0;i<constraints.size();i++)
	{
		Particle *a=constraints[i]->jun;
		Particle *b=constraints[i]->sen;

		double vxD=a->vel[0]-b->vel[0];
		double vyD=a->vel[1]-b->vel[1];

		jacobianV[i]=2*(vxD*vxD+vyD*vyD);
	}
}

void ConstraintSystem::GetHarmonicOsciliator( double *harmonic, ParticleList *particles )
{
	for (int i=0;i<constraints.size();i++)
	{
		Particle *a=constraints[i]->jun;
		Particle *b=constraints[i]->sen;

		double vxD=a->vel[0]-b->vel[0];
		double vyD=a->vel[1]-b->vel[1];

		double xd=b->pos[0]-a->pos[0];
		double yd=b->pos[1]-a->pos[1];

		double C1=2*((b->pos[0]-a->pos[0])*(b->vel[0]-a->vel[0])+(b->pos[1]-a->pos[1])*(b->vel[1]-a->vel[1]));
		double C0=xd*xd+yd*yd-constraints[i]->distance*constraints[i]->distance;

		harmonic[i]=-2*(vxD*vxD+vyD*vyD)-alpha*C1-beta*C0;
	}
}

int ConstraintSystem::GetScale()
{
	return constraints.size();
}

Constraint::Constraint( Particle *a, Particle *b, double distance )
{
	if (a->index<b->index)
	{
		jun=a;
		sen=b;
	}else{
		jun=b;
		sen=a;
	}

	this->distance=distance;
}
